#ifndef PCL_NO_PRECOMPILE
#define PCL_NO_PRECOMPILE
#endif

#include <memory>
#include <chrono>
#include <string>
#include <boost/make_shared.hpp>
#include <vector>
#include <cmath>
#include "rclcpp/rclcpp.hpp"
//#include "std_msgs/msg/string.hpp"
#include <sensor_msgs/msg/laser_scan.hpp>
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "std_msgs/msg/header.hpp"

#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
//#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
//#include <pcl/octree/octree.h>
#include <pcl/kdtree/kdtree_flann.h>
//#include <pcl/registration/icp.h>
//#include <pcl/features/normal_3d.h>

// Eigen
#include <Eigen/Core>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
using namespace std;
#define SCAN_CLOUD 1
class BlueseaSubScan: public rclcpp::Node
{
    public:
        BlueseaSubScan(): Node("process_bluesea")
        {
            using std::placeholders::_1;
            if(SCAN_CLOUD==0)
            sub_bluesea_cloud = this->create_subscription<sensor_msgs::msg::PointCloud2>(
                "/cloud", 
                10, std::bind(&BlueseaSubScan::topic_callback_cloud, this, std::placeholders::_1));
            if(SCAN_CLOUD==1)
            sub_bluesea_scan = this->create_subscription<sensor_msgs::msg::LaserScan>(
                "/scan", 
                10, std::bind(&BlueseaSubScan::topic_callback_scan, this, std::placeholders::_1));
            publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("/my_topic", 10);
        }
    private:
     void topic_callback_cloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg) 
     {
         size_t size;
         double x,y,max_x,max_y;
         pcl::PointCloud<pcl::PointXYZ> lidar_cloud;
         sensor_msgs::msg::PointCloud2 mymsg;
         pcl::fromROSMsg(*msg, lidar_cloud);
            size=lidar_cloud.points.size();
       // for(int32_t i=0;i<20;i++)    
       //   RCLCPP_INFO(this->get_logger(),"i=%d  x=%5.2f y=%5.2f,z=%5.2f",i,lidar_cloud.points[i].x,lidar_cloud.points[i].y,lidar_cloud.points[i].z);
        pcl::PointCloud<pcl::PointXYZ>::Ptr trybb_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        for(auto &point:lidar_cloud.points)
         {
            pcl::PointXYZ &p = point;
            if(p.x>0)
            {
                p.z=0.3;
                trybb_cloud->push_back(p);
            }
         }
            pcl::toROSMsg(*trybb_cloud, mymsg);
            mymsg.header.stamp = now();
            mymsg.header.frame_id="map";
            publisher_->publish(mymsg);
     }
    void topic_callback_scan(const sensor_msgs::msg::LaserScan::SharedPtr scan) 
     {
       int num_points = scan->ranges.size();
         sensor_msgs::msg::PointCloud2 mymsg;
       // 初始化点云
        pcl::PointCloud<pcl::PointXYZ> lidar_cloud;
        pcl::PointXYZ point_temp;
      // lidar_cloud.resize(num_points);

        for (size_t i = 0; i < num_points; ++i)
        {
            double angle = scan->angle_min + i * (scan->angle_increment) ;
            double distance = scan->ranges[i];
            double dist_cha;
            if(count!=0)
            {
              dist_cha=fabs(last_distance[i]-distance);
              if(dist_cha<1)
                if(max_distance<dist_cha) max_distance=dist_cha;
              if(dist_cha>0.005)sum++;
            }
            last_distance[i]=distance;
            if(distance>0.3)
            {
            // 将角度转换为x,y坐标（假设激光雷达垂直于地面）
            float x = distance * cos(angle);
            float y = distance * sin(angle);

            // 添加z坐标通常为0，因为激光雷达水平扫描
            float z = 0.2;

            // 将点添加到点云中point_temp
            point_temp.x=x;
            point_temp.y=y;
            point_temp.z=z;
            lidar_cloud.push_back(point_temp);
            }
            count++;
        }
          pcl::toROSMsg(lidar_cloud, mymsg);
            mymsg.header.stamp =now();
            mymsg.header.frame_id="map";
            publisher_->publish(mymsg);
             RCLCPP_INFO(this->get_logger(),"have  a new scan!=%f cha=%f  sum=%d",scan->angle_increment,max_distance,sum);
             max_distance=0.0;
              sum=0;
    }
    
    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_bluesea_cloud;
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr sub_bluesea_scan;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher_;
    int count=0;float max_distance=0.0;
    int sum=0;
    float last_distance[2000];
};


int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<BlueseaSubScan>());
  
  rclcpp::shutdown();
  return 0;
}







